#include "stm32f1xx_hal.h"
#include "MPU6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "Motor.h"
#include "Encoder.h"
extern TIM_HandleTypeDef htim2;                                //外部引用定时器4
extern TIM_HandleTypeDef htim4;                                //外部引用定时器2
float Pitch, Roll, Yaw;                                        //欧拉角
short gyrox, gyroy, gyroz;                                     //三轴角速度
int Encoder_Sum;                                               //速度环积分值
float ZhiLiHuan_Kp = 7.2, ZhiLiHuan_Kd = 0.028;                //直立环PD控制
float SpeedHuan_Kp = -0.35, SpeedHuan_Ki;                      //速度环PI控制，其中P的极性为-
float TurnningHuan_Kp = 3;                                     //转向环P控制器

int ZhiLiHuan_Output, SpeedHuan_Output, TurnningHuan_Output;   //直立环输出，速度环输出，转向环输出
int Target_Speed, Target_turn = 0;                             //目标速度，目标转向角度
float Middle_Angle = 2.5;                                      //机械中值，即当小车平衡的时候，roll的数值
int Encoder_Left, Encoder_Right;                               //左编码器读数，右编码器读数
//直立环PD控制器
//输入：期望角度、真实角度、角速度
int ZhiLiHuan_Control(float MiddleAngle, float RealAngle, float gyro_X)
{
	int OutPut;
	OutPut = ZhiLiHuan_Kp * (RealAngle - MiddleAngle) + ZhiLiHuan_Kd * gyro_X;
	return OutPut;
}

//速度环PI控制器
//输入：期望速度、左编码器、右编码器
int SpeedHuan_Control(int Target_Speed,int encoder_L,int encoder_R)
{
	static int Error_DiTong_Last;                                  //前次低通滤波输出值
	static float DiTong_Rate = 0.7;                                //低通滤波a值，取经验值为0.7
	int Error_Now, Err_LowOut, OutPut;                             //本次速度误差值，本次低通滤波后的误差值，速度环输出值
	SpeedHuan_Ki = SpeedHuan_Kp / 200;                             //ki取经验值 为 kp/200
	//1、计算偏差值
	Error_Now = (encoder_L + encoder_R) - Target_Speed;
	//2、对本次误差值进行低通滤波
	Err_LowOut = (1 - DiTong_Rate) * Error_Now + DiTong_Rate * Error_DiTong_Last;
	Error_DiTong_Last = Err_LowOut;                                //将本次低通滤波输出值记录下来
	//3、积分 
	Encoder_Sum += Err_LowOut;                                     //编码器积分值等于其自身累加
	//4、积分限幅(-20000~20000) 
	if(Encoder_Sum > 20000) Encoder_Sum = 20000;
	if(Encoder_Sum < -20000) Encoder_Sum = -20000;
	//5、速度环计算
	OutPut = SpeedHuan_Kp * Err_LowOut + SpeedHuan_Ki * Encoder_Sum;//Kp * 速度偏差滤波值 + Ki * 真实速度积分
	return OutPut;
}

//转向环PD控制器，用于简单的差速转向原理 
int TurnningHuan_Control(float Yaw, int TargetSpeed_turn) 
{
	int OutPut;
	OutPut = TurnningHuan_Kp * (Yaw - TargetSpeed_turn);  //将偏航角减去目标转向值得到误差，再与Kp相乘
	return OutPut;
}

void PID_Control(void)	//将这段函数mpu6050通过自带的INT中断口 每隔10ms调用一次，或者5ms，取决于陀螺仪读取频率
{
	int PID_out;                                        //直立环与速度环联合作用下的输出值
	int Motor1, Motor2;                                 //电机PWM输入
	//1、读取编码器和陀螺仪的数据
	Encoder_Left = Encoder_GetSpeed(&htim2);            //从编码器读取左电机速度值
	Encoder_Right = -Encoder_GetSpeed(&htim4);          //从编码器读取右电机速度值
	mpu_dmp_get_data(&Pitch, &Roll, &Yaw);              //从mpu6050读取角度值
	MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);          //从mpu6050读取角速度值
	
	//2、将数据传入PID控制器，计算输出结果，即左右电机转速值
	SpeedHuan_Output = SpeedHuan_Control(Target_Speed, Encoder_Left, Encoder_Right);   //PI速度环控制器输出角度值
	ZhiLiHuan_Output = ZhiLiHuan_Control(SpeedHuan_Output + Middle_Angle, Roll, gyrox);//PD直立环输出PWM Med_Angle 即机械中值，加上平衡时角度偏移量用以修正
	TurnningHuan_Output = TurnningHuan_Control(Yaw, Target_turn);
	PID_out = ZhiLiHuan_Output;             //PD转向环输出PWM 
	Motor1 = PID_out - TurnningHuan_Output; //左右电机的PWM 一边减去一边加上  便形成了差速转向
	Motor2 = PID_out + TurnningHuan_Output; 
	Speed_Limit(&Motor1 ,&Motor2);          //给电机进行了一个限幅，避免因积分累加导致速度过载过载
	Motor_SetSpeed(Motor1, Motor2);         //最终PWM驱动电机
//	if(Roll > 25 || Roll < -25)
//	{
//		Motor_SetSpeed(0, 0);
//		Encoder_Sum = 0;
//	}
} 

